"""
文件名: ibc_nav.py

描述:
这个模块定义了一个名为IBCNav的类，用于解析和处理来自惯性/北斗组合（IBC）导航系统的数据。
它能够处理两种类型的数据：融合数据（Fusion Data）和原始数据（Origin Data）。

主要功能:
1. 解析二进制数据流，识别并提取有效的数据包
2. 将二进制数据解析为结构化的Python字典
3. 提供接口来访问最新的融合数据和原始数据
4. 维护数据更新标志，指示新数据的到达

主要类: IBCNav

类属性:
- _ENABLE_MOBILE_PLATFORM: 控制是否启用移动平台特定的功能
- _DEBUG_ENABLE: 控制是否启用调试输出
- __HEAD, __MAX_LEN, __FUSION_ID, __ORIGIN_ID: 用于数据包识别的常量
- default_fusion_data, get_default_origin_data: 默认数据结构

主要方法:
- __init__(): 初始化IBCNav对象
- push_data(data): 向解析器推送新的数据
- parse_fusion_data(data): 解析融合数据包
- parse_origin_data(data): 解析原始数据包
- get_fusion_data(): 获取最新的融合数据
- get_origin_data(): 获取最新的原始数据
- get_fusion_data_by_key(key): 获取特定的融合数据项
- get_origin_data_by_key(key): 获取特定的原始数据项
- get_update_flag(): 获取数据更新标志
- get_origin_keys(): 获取原始数据的所有键
- get_fusion_keys(): 获取融合数据的所有键

辅助函数:
- byte_to_hex(data): 将字节数组转换为十六进制字符串
- hex_to_byte(hex_str): 将十六进制字符串转换为字节数组

使用方法:
1. 创建IBCNav对象
2. 使用push_data()方法推送原始二进制数据
3. 使用get_fusion_data()或get_origin_data()获取解析后的数据
4. 使用get_update_flag()检查是否有新数据到达

数据格式:
- 融合数据包（FUSION_ID = 0xD1）：包含导航状态、姿态、位置、速度等信息
- 原始数据包（ORIGIN_ID = 0xDA）：包含原始传感器数据，如IMU、磁力计、GPS等

注意事项:
- 输入数据应为二进制格式
- 该模块假设输入数据采用小端字节序
- 部分数据项在解析时进行了单位转换（如经纬度、高度等）

依赖:
- struct 模块：用于二进制数据的打包和解包

测试:
模块末尾包含一个简单的测试，演示了如何使用IBCNav类处理样例数据。

作者: [IBC-kishs]
创建日期: [24-7-10]
最后修改日期: [24-6-16]
版本: 1.1
"""
import struct

class IBCNav:
    # Constants
    _ENABLE_MOBILE_PLATFORM = 0 # 使能动平台数据
    _DEBUG_ENABLE = False

    __HEAD = 0x90EB
    __MAX_LEN = 200
    __FUSION_ID = 0xD1
    __ORIGIN_ID = 0xDA

    __PLATFORM_STRUCT_FORMAT = '<HBBIBfffffffiiiiifffffffBHHBBBBbhhBBBfffHBHBfffHHHH'
    __NORMAL_STRUCT_FORMAT = '<HBBIBfffffffiiiiifffffffBHHBBBBbhhBBBfffHBHBHH'

    default_fusion_data = {
        'count': 0,
        'state': 0,
        'pitch': 0.0,
        'roll': 0.0,
        'yaw': 0.0,
        'yaw_gps': 0.0,
        'pitch_rate': 0.0,
        'roll_rate': 0.0,
        'yaw_rate': 0.0,
        'lon': 0.0,
        'lat': 0.0,
        'alt_baro': 0.0,
        'alt_gps': 0.0,
        'alt': 0.0,
        'velocity_x': 0.0,
        'velocity_y': 0.0,
        'velocity_z': 0.0,
        'velocity_air': 0.0,
        'accel_x': 0.0,
        'accel_y': 0.0,
        'accel_z': 0.0,
        'satellite_num': 0,
        'hdop': 0.0,
        'vdop': 0.0,
        'gps_status': 0,
        'gps_hh': 0,
        'gps_mm': 0,
        'gps_ss': 0,
        'temperature': 0,
        'HDT': 0.0,
        'HDG_Dev': 0.0,
        'redundancy': 0,
        'GPS0_DT': 0,
        'GPS1_DT': 0,
        'GPS_vx': 0.0,
        'GPS_vy': 0.0,
        'GPS_vz': 0.0,
        'gps_ms': 0,
        'gps_day': 0,
        'gps_week': 0,
        'ahrs_state': 0,
        'eastward_dis': 0.0,
        'northward_dis': 0.0,
        'skyward_dis': 0.0,
        'std_dev': 0.0,
        'std_dev_up': 0.0,
        'Engine_RPM': 0.0
    }

    default_origin_data = {
        'count': 0,
        'DIAG_STAT': 0,
        'mainIMU_WX': 0.0,
        'mainIMU_WY': 0.0,
        'mainIMU_WZ': 0.0,
        'mainIMU_FX': 0.0,
        'mainIMU_FY': 0.0,
        'mainIMU_FZ': 0.0,
        'backupIMU_WX': 0.0,
        'backupIMU_WY': 0.0,
        'backupIMU_WZ': 0.0,
        'backupIMU_FX': 0.0,
        'backupIMU_FY': 0.0,
        'backupIMU_FZ': 0.0,
        'MAG_HX': 0.0,
        'MAG_HY': 0.0,
        'MAG_HZ': 0.0,
        'GPS_sec': 0.0,
        'heading': 0,
        'heading_type': 0,
        'hor_spd': 0.0,
        'trk_gnd': 0.0,
        'vert_spd': 0.0,
        'lat': 0.0,
        'lon': 0.0,
        'hgt': 0.0,
        'pos_type': 0,
        'PPS_dt': 0.0
    }

    def __init__(self):
        self.fusionData = self.default_fusion_data.copy()
        self.fusion_update_flag = False
        self.originData = self.default_origin_data.copy()
        self.origin_update_flag = False

        self._state = 0
        self._TempHeader = 0
        self._TempLen = 0
        self._TempLoc = 0
        self._TempBuf = bytearray(self.__MAX_LEN)

        self.__PLATFORM_STRUCT_FORMAT_LEN = struct.calcsize(self.__PLATFORM_STRUCT_FORMAT)
        self.__NORMAL_STRUCT_FORMAT_LEN = struct.calcsize(self.__NORMAL_STRUCT_FORMAT)

    @staticmethod
    def _checksum(data):
        return sum(data) & 0xFFFF

    def __push_one_data(self, data):
        ret = False

        if self._state == 0:
            self._TempHeader = (self._TempHeader >> 8) | (data << 8)
            if self._TempHeader == self.__HEAD:
                self._state = 1
                self._TempBuf[0] = self._TempHeader & 0xFF
                self._TempBuf[1] = data
                self._TempHeader = 0

        elif self._state == 1:
            self._TempLen = data
            self._TempBuf[2] = data
            if self._TempLen < self.__MAX_LEN:
                self._TempLoc = 3
                self._state = 2
            else:
                self._state = 0

        elif self._state == 2:
            if self._TempLoc < (self._TempLen - 1):
                self._TempBuf[self._TempLoc] = data
                self._TempLoc += 1
            else:
                self._TempBuf[self._TempLoc] = data
                self._TempLoc += 1
                if self._DEBUG_ENABLE:
                    print("self._TempBuf: ", byte_to_hex(self._TempBuf))
                checksum = (self._TempBuf[self._TempLen - 1]
                            << 8) | self._TempBuf[self._TempLen - 2]
                if checksum == self._checksum(self._TempBuf[:self._TempLen - 2]):
                    if self._TempBuf[3] == self.__FUSION_ID:
                        self.parse_fusion_data(self._TempBuf[:self._TempLen])
                        if self._DEBUG_ENABLE:
                            self.prfFusionData()
                    elif self._TempBuf[3] == self.__ORIGIN_ID:
                        self.parse_origin_data(self._TempBuf[:self._TempLen])
                        if self._DEBUG_ENABLE:
                            self.prfOriginData()
                    ret = True
                else:
                    if self._DEBUG_ENABLE:
                        print("Checksum error!")

                self._TempLoc = 0
                self._state = 0
        return ret

    def push_data(self, data):
        ret = False
        if type(data) == bytearray or type(data) == bytes:
            for i in range(len(data)):
                ret = ret or self.__push_one_data(data[i])
        elif type(data) == int:
            ret = self.__push_one_data(data)
        else:
            if self._DEBUG_ENABLE:
                print("Invalid data type!")
        return ret

    def parse_fusion_data(self, data):
        # Define the format string based on the C struct
        # '<' means little-endian
        # 'H' for uint16_t, 'B' for uint8_t, 'f' for float, 'i' for int32_t, 'h' for int16_t

        if self.__PLATFORM_STRUCT_FORMAT_LEN == len(data):
            format_string = self.__PLATFORM_STRUCT_FORMAT
        elif self.__NORMAL_STRUCT_FORMAT_LEN == len(data):
            format_string = self.__NORMAL_STRUCT_FORMAT
        else:
            print("Invalid data length!")

        try:
            # Unpack the binary data
            unpacked_data = struct.unpack(format_string, data)

            # Create a dictionary with the unpacked data
            fusion_data = {
                # 'head': unpacked_data[0],
                # 'len': unpacked_data[1],
                # 'id': unpacked_data[2],
                'count': unpacked_data[3],
                'state': unpacked_data[4],
                'pitch': unpacked_data[5],
                'roll': unpacked_data[6],
                'yaw': unpacked_data[7],
                'yaw_gps': unpacked_data[8],
                'pitch_rate': unpacked_data[9],
                'roll_rate': unpacked_data[10],
                'yaw_rate': unpacked_data[11],
                'lon': unpacked_data[12] * 1e-7,  # Convert to degrees
                'lat': unpacked_data[13] * 1e-7,  # Convert to degrees
                'alt_baro': unpacked_data[14] / 100,  # Convert to meters
                'alt_gps': unpacked_data[15] / 100,  # Convert to meters
                'alt': unpacked_data[16] / 100,  # Convert to meters
                'velocity_x': unpacked_data[17],
                'velocity_y': unpacked_data[18],
                'velocity_z': unpacked_data[19],
                'velocity_air': unpacked_data[20],
                'accel_x': unpacked_data[21],
                'accel_y': unpacked_data[22],
                'accel_z': unpacked_data[23],
                'satellite_num': unpacked_data[24],
                'hdop': unpacked_data[25] / 100,  # Convert to meters
                'vdop': unpacked_data[26] / 100,  # Convert to meters
                'gps_status': unpacked_data[27],
                'gps_hh': unpacked_data[28],
                'gps_mm': unpacked_data[29],
                'gps_ss': unpacked_data[30],
                'temperature': unpacked_data[31],
                'HDT': unpacked_data[32] / 10,  # Convert to degrees
                'HDG_Dev': unpacked_data[33] / 10,  # Convert to degrees
                'redundancy': unpacked_data[34],
                'GPS0_DT': unpacked_data[35] * 100,  # Convert to milliseconds
                'GPS1_DT': unpacked_data[36] * 100,  # Convert to milliseconds
                'GPS_vx': unpacked_data[37],
                'GPS_vy': unpacked_data[38],
                'GPS_vz': unpacked_data[39],

                'gps_ms': unpacked_data[40],
                'gps_day': unpacked_data[41],
                'gps_week': unpacked_data[42],
                'ahrs_state': unpacked_data[43],
            }

            if self.__PLATFORM_STRUCT_FORMAT_LEN == len(data):
                fusion_data['eastward_dis'] = unpacked_data[44]
                fusion_data['northward_dis'] = unpacked_data[45]
                fusion_data['skyward_dis'] = unpacked_data[46]

                fusion_data['std_dev'] = unpacked_data[47] * 0.01
                fusion_data['std_dev_up'] = unpacked_data[48] * 0.01  # Convert to meters

                fusion_data['Engine_RPM'] = unpacked_data[49]

            elif self.__NORMAL_STRUCT_FORMAT_LEN == len(data):
                fusion_data['Engine_RPM'] = unpacked_data[44]

            self.fusionData = fusion_data
            self.fusion_update_flag = True
        except Exception as e:
            print(f"Error parsing fusion data: {e}")
            fusion_data = {}

        return fusion_data

    def parse_origin_data(self, data):
        # Define the format string based on the C struct
        # '<' means little-endian
        # 'H' for uint16_t, 'B' for uint8_t, 'I' for uint32_t, 'f' for float, 'd' for double
        format_string = '<HBBIIffffffffffffffffdIddddddIIdddhffH'

        try:
            # Unpack the binary data
            unpacked_data = struct.unpack(format_string, data)

            # Create a dictionary with the unpacked data
            origin_data = {
                # 'head': unpacked_data[0],
                # 'len': unpacked_data[1],
                # 'id': unpacked_data[2],
                'count': unpacked_data[3],
                'DIAG_STAT': unpacked_data[4],
                'mainIMU_WX': unpacked_data[5],
                'mainIMU_WY': unpacked_data[6],
                'mainIMU_WZ': unpacked_data[7],
                'mainIMU_FX': unpacked_data[8],
                'mainIMU_FY': unpacked_data[9],
                'mainIMU_FZ': unpacked_data[10],
                'backupIMU_WX': unpacked_data[11],
                'backupIMU_WY': unpacked_data[12],
                'backupIMU_WZ': unpacked_data[13],
                'backupIMU_FX': unpacked_data[14],
                'backupIMU_FY': unpacked_data[15],
                'backupIMU_FZ': unpacked_data[16],
                'MAG_HX': unpacked_data[17],
                'MAG_HY': unpacked_data[18],
                'MAG_HZ': unpacked_data[19],
                'GPS_sec': unpacked_data[20],
                'heading': unpacked_data[21],
                'heading_type': unpacked_data[22],
                'hor_spd': unpacked_data[23],
                'trk_gnd': unpacked_data[24],
                'vert_spd': unpacked_data[25],
                'lat': unpacked_data[26],
                'lon': unpacked_data[27],
                'hgt': unpacked_data[28],
                'pos_type': unpacked_data[29],
                'PPS_dt': unpacked_data[30],
            }

            self.originData = origin_data
            self.origin_update_flag = True
        except Exception as e:
            print(f"Error parsing origin data: {e}")

    def prfOriginData(self):
        print("\n\nOriginData:")
        update_flag, data = self.get_origin_data()
        print(f"Update flag: {update_flag}")
        for key, value in data.items():
            print(f"{key}: {value}")

    def prfFusionData(self):
        print("\n\nFusionData:")
        update_flag, data = self.get_fusion_data()
        print(f"Update flag: {update_flag}")
        for key, value in data.items():
            print(f"{key}: {value}")

    def get_fusion_data(self):
        update_flag = self.fusion_update_flag
        self.fusion_update_flag = False
        return update_flag, self.fusionData

    def get_fusion_data_by_key(self, key):
        return self.fusionData.get(key)

    def get_fusion_update_flag(self):
        return self.fusion_update_flag

    def get_origin_data(self):
        update_flag = self.origin_update_flag
        self.origin_update_flag = False
        return update_flag, self.originData

    def get_origin_data_by_key(self, key):
        return self.originData.get(key)

    def get_origin_update_flag(self):
        return self.origin_update_flag

    def get_update_flag(self):
        return self.fusion_update_flag, self.origin_update_flag

    def get_origin_keys(self):
        return self.originData.keys()

    def get_fusion_keys(self):
        return self.fusionData.keys()


def byte_to_hex(data: bytearray):
    """将字节转换为16进制字符串"""
    return ' '.join(["%02X" % x for x in data]) if data else None


def hex_to_byte(hex_str: str):
    """将16进制字符串转换为字节"""
    return bytearray.fromhex(hex_str) if hex_str else None


# Main execution
if __name__ == "__main__":
    # 创建IBCNav对象实例
    nav = IBCNav()

    # 测试数据
    # strD1: 融合数据包的十六进制字符串表示
    # 此数据包包含导航状态、姿态、位置等信息
    strD1 = "EB 90 7C D1 00 00 00 00 01 E6 FD 8E 3C E6 FD 0E BD CD B1 95 40 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 F2 79 58 45 5A AB D3 17 9C 18 00 00 9C 18 00 00 9C 18 00 00 00 00 80 3F 00 00 00 C0 00 00 40 40 00 00 00 00 00 00 80 3F 00 00 80 3F CD CC 1C 41 0F 00 00 00 00 03 08 03 05 19 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 CD CC CC 3D 00 00 00 DC 08 02 00 00 C0 19"

    # strDA: 原始数据包的十六进制字符串表示
    # 此数据包包含原始传感器数据，如IMU、磁力计、GPS等信息
    strDA = "EB 90 B4 DA 00 00 00 00 01 00 00 00 00 00 80 3F 00 00 00 40 00 00 40 40 00 00 80 40 00 00 A0 40 00 00 C0 40 00 00 E0 40 00 00 00 41 00 00 10 41 00 00 20 41 00 00 30 41 00 00 40 41 00 00 50 41 00 00 60 41 00 00 70 41 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 1B 1F 77 5B F3 03 65 40 6D 7C DC 6D CD 8F 42 40 00 00 00 00 00 80 50 40 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 D8 13"

    # 将十六进制字符串转换为字节数组
    # replace(" ", "") 移除字符串中的所有空格
    # bytes.fromhex() 将十六进制字符串转换为字节对象
    data_D1 = bytes.fromhex(strD1.replace(" ", ""))
    data_DA = bytes.fromhex(strDA.replace(" ", ""))

    print("\nTesting Fusion Data (D1):")
    # 将融合数据推送到导航对象
    nav.push_data(data_D1)
    # 获取并打印解析后的融合数据
    update_flag, fusion_data = nav.get_fusion_data()
    print(f"Update flag: {update_flag}")
    for key, value in fusion_data.items():
        print(f"{key}: {value}")

    print("\nTesting Origin Data (DA):")
    # 将原始数据逐字节推入推送到导航对象
    for byte in data_DA:
        result = nav.push_data(byte)
    # 获取并打印解析后的原始数据
    update_flag, origin_data = nav.get_origin_data()
    print(f"Update flag: {update_flag}")
    for key, value in origin_data.items():
        print(f"{key}: {value}")

    # 测试获取特定键的数据
    print("\nTesting get_fusion_data_by_key:")
    print(f"Latitude: {nav.get_fusion_data_by_key('lat')}")
    print(f"Longitude: {nav.get_fusion_data_by_key('lon')}")

    print("\nTesting get_origin_data_by_key:")
    print(f"Main IMU WX: {nav.get_origin_data_by_key('mainIMU_WX')}")
    print(f"Main IMU WY: {nav.get_origin_data_by_key('mainIMU_WY')}")

    # 打印所有可用的键
    print("\nAvailable Fusion Data Keys:")
    print(nav.get_fusion_keys())

    print("\nAvailable Origin Data Keys:")
    print(nav.get_origin_keys())

    # 获取更新标志位,由于数据已经被取出，所以为均为False
    fusion_update_flag, origin_update_flag = nav.get_update_flag()
    print(f"Fusion update flag: {fusion_update_flag}, Origin update flag: {origin_update_flag}")